#include "Chassis_Control.h"

uint8_t Car_GoForeward_Flag,Car_GoReserve_Flag,Car_GoLeft_Flag,Car_GoRight_Flag;
/**
 *  @brief 左前轮正转
 */
void LF_GoForeward(void)
{
	HAL_GPIO_WritePin(MOTOR1_PORT,MOTOR1_A_PIN,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(MOTOR1_PORT,MOTOR1_B_PIN,GPIO_PIN_SET);
}

/**
 *  @brief 左前轮反转
 */
void LF_GoReserve(void)
{
	HAL_GPIO_WritePin(MOTOR1_PORT,MOTOR1_A_PIN,GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR1_PORT,MOTOR1_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 左前轮停止
 */
void LF_Stop(void)
{
	HAL_GPIO_WritePin(MOTOR1_PORT,MOTOR1_A_PIN|MOTOR1_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 右前轮正转
 */
void RF_GoForeward(void)
{
	HAL_GPIO_WritePin(MOTOR2_PORT,MOTOR2_A_PIN,GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR2_PORT,MOTOR2_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 右前轮反转
 */
void RF_GoReserve(void)
{
	HAL_GPIO_WritePin(MOTOR2_PORT,MOTOR2_A_PIN,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(MOTOR2_PORT,MOTOR2_B_PIN,GPIO_PIN_SET);
}

/**
 *  @brief 右前轮停止
 */
void RF_Stop(void)
{
	HAL_GPIO_WritePin(MOTOR2_PORT,MOTOR2_A_PIN|MOTOR2_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 左后轮正转
 */
void LB_GoForeward(void)
{
	HAL_GPIO_WritePin(MOTOR3_PORT,MOTOR3_A_PIN,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(MOTOR3_PORT,MOTOR3_B_PIN,GPIO_PIN_SET);
}

/**
 *  @brief 左后轮反转
 */
void LB_GoReserve(void)
{
	HAL_GPIO_WritePin(MOTOR3_PORT,MOTOR3_A_PIN,GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR3_PORT,MOTOR3_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 左后轮停止
 */
void LB_Stop(void)
{
	HAL_GPIO_WritePin(MOTOR3_PORT,MOTOR3_A_PIN|MOTOR3_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 右后轮正转
 */
void RB_GoForeward(void)
{
	HAL_GPIO_WritePin(MOTOR4_PORT,MOTOR4_A_PIN,GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR4_PORT,MOTOR4_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 右后轮反转
 */
void RB_GoReserve(void)
{
	HAL_GPIO_WritePin(MOTOR4_PORT,MOTOR4_A_PIN,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(MOTOR4_PORT,MOTOR4_B_PIN,GPIO_PIN_SET);
}

/**
 *  @brief 右后轮停止
 */
void RB_Stop(void)
{
 	HAL_GPIO_WritePin(MOTOR4_PORT,MOTOR4_A_PIN|MOTOR4_B_PIN,GPIO_PIN_RESET);
}

/**
 *  @brief 整车前进
 */
void Car_GoForeward(void)
{
	LF_GoForeward();
	RF_GoForeward();
	LB_GoForeward();
	RB_GoForeward();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车后退
 */
void Car_GoReserve(void)
{
	LF_GoReserve();
	RF_GoReserve();
	LB_GoReserve();
	RB_GoReserve();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车向左平移
 */
void Car_GoLeft(void)
{
	LF_GoReserve();
	RF_GoForeward();
	LB_GoForeward();
	RB_GoReserve();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车向右平移
 */
void Car_GoRight(void)
{
	LF_GoForeward();
	RF_GoReserve();
	LB_GoReserve();
	RB_GoForeward();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车顺时针旋转
 */
void Car_TurnClockwise(void)
{
	LF_GoForeward();
	RF_GoReserve();
	LB_GoForeward();
	RB_GoReserve();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车逆时针旋转
 */
void Car_TurnAnticlockwise(void)
{
	LF_GoReserve();
	RF_GoForeward();
	LB_GoReserve();
	RB_GoForeward();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,7500);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,7500);
}

/**
 *  @brief 整车停止
 */
void CarStop(void)
{
  LF_Stop();
	RF_Stop();
	LB_Stop();
	RB_Stop();
	
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR1_PWM_CHANNEL,0);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR2_PWM_CHANNEL,0);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR3_PWM_CHANNEL,0);
	__HAL_TIM_SET_COMPARE(&htim2,MOTOR4_PWM_CHANNEL,0);

}
